/**
 * @author UCSD MOOC development team and YOU
 * 
 * A class which reprsents a graph of geographic locations
 * Nodes in the graph are intersections between 
 *
 */
package roadgraph;


import java.util.ArrayList;
import java.util.Comparator;
import java.util.HashMap;
import java.util.HashSet;
import java.util.LinkedList;
import java.util.List;
import java.util.Set;
import java.util.Stack;
import java.util.Map;
import java.util.PriorityQueue;
import java.util.Queue;
import java.util.function.Consumer;
import java.lang.Math;
import geography.GeographicPoint;
import util.GraphLoader;

/**
 * @author UCSD MOOC development team and YOU
 * 
 * A class which represents a graph of geographic locations
 * Nodes in the graph are intersections between 
 *
 */
public class MapGraph {
	private HashMap<GeographicPoint, Intersection> nodes;
	private int numNodes = 0;
	private int numEdges = 0;
	
	/** 
	 * Create a new empty MapGraph 
	 */
	public MapGraph()
	{
		nodes = new HashMap<>();
	}
	
	/**
	 * Get the number of vertices (road intersections) in the graph
	 * @return The number of vertices in the graph.
	 */
	public int getNumVertices()
	{
		return this.numNodes;
	}
	
	/**
	 * Return the intersections, which are the vertices in this graph.
	 * @return The vertices in this graph as GeographicPoints
	 */
	public Set<GeographicPoint> getVertices()
	{	
		return nodes.keySet();
	}
	
	/**
	 * Get the number of road segments in the graph
	 * @return The number of edges in the graph.
	 */
	public int getNumEdges()
	{
		return this.numEdges;
	}

	
	
	/** Add a node corresponding to an intersection at a Geographic Point
	 * If the location is already in the graph or null, this method does 
	 * not change the graph.
	 * @param location  The location of the intersection
	 * @return true if a node was added, false if it was not (the node
	 * was already in the graph, or the parameter is null).
	 */
	public boolean addVertex(GeographicPoint location)
	{
		if (nodes.containsKey(location)) {
			return false;
		}else {
			Intersection nIntersection = new Intersection(this.numNodes+1, location);
			nodes.put(location, nIntersection);
			this.numNodes++;
			return true;
		}
	}
	
	/**
	 * Adds a directed edge to the graph from pt1 to pt2.  
	 * Precondition: Both GeographicPoints have already been added to the graph
	 * @param from The starting point of the edge
	 * @param to The ending point of the edge
	 * @param roadName The name of the road
	 * @param roadType The type of the road
	 * @param length The length of the road, in km
	 * @throws IllegalArgumentException If the points have not already been
	 *   added as nodes to the graph, if any of the arguments is null,
	 *   or if the length is less than 0.
	 */
	public void addEdge(GeographicPoint from, GeographicPoint to, String roadName,
			String roadType, double length) throws IllegalArgumentException {
		if (!nodes.containsKey(from) | !nodes.containsKey(to)) {
			throw new IllegalArgumentException("Start Node or End Node does not exist.");
		}
		if (roadName==null | roadType==null | length<0) {
			throw new IllegalArgumentException("RoadName, RoadType or Length is illegal.");
		}
		
		Intersection fromNode = nodes.get(from);
		Intersection toNode = nodes.get(to);
		Road road = new Road(roadName, roadType, length, fromNode, toNode);
		fromNode.addOutEdge(road);
		numEdges++;
	}
	

	/** Find the path from start to goal using breadth first search
	 * 
	 * @param start The starting location
	 * @param goal The goal location
	 * @return The list of intersections that form the shortest (unweighted)
	 *   path from start to goal (including both start and goal).
	 */
	public List<GeographicPoint> bfs(GeographicPoint start, GeographicPoint goal) {
		// Dummy variable for calling the search algorithms
        Consumer<GeographicPoint> temp = (x) -> {};
        return bfs(start, goal, temp);
	}
	
	/** Find the path from start to goal using breadth first search
	 * 
	 * @param start The starting location
	 * @param goal The goal location
	 * @param nodeSearched A hook for visualization.  See assignment instructions for how to use it.
	 * @return The list of intersections that form the shortest (unweighted)
	 *   path from start to goal (including both start and goal).
	 */
	public List<GeographicPoint> bfs(GeographicPoint start, 
			 					     GeographicPoint goal, Consumer<GeographicPoint> nodeSearched)
	{
		Queue<Intersection> queue = new LinkedList<>();
		HashSet<Intersection> visted = new HashSet<>();
		HashMap<Intersection, Intersection> map = new HashMap<>();
		
		List<GeographicPoint> pathNodes = new ArrayList<>();
		
		queue.add(nodes.get(start));
		visted.add(nodes.get(start));
		boolean found = false;
		while (!queue.isEmpty()) {
			
			Intersection curr = queue.poll();
			GeographicPoint next = curr.getLoacation();
			nodeSearched.accept(next);
			
			if(curr == nodes.get(goal)){
				found = true;
				break;
			}
			else {
				for (Intersection intersection : curr.neighbors()) {
					if (!visted.contains(intersection)) {
						visted.add(intersection);
						queue.add(intersection);
						map.put(intersection, curr);
					}
				}
			}
			
		}
		
			
		
		

		if (found) {
			Intersection g = nodes.get(goal);              
			Stack<GeographicPoint> stack = new Stack<>();    //use stack to inverse the hop sequence.
			stack.push(g.getLoacation());
			while (true) {
				Intersection s = map.get(g);
				stack.push(s.getLoacation());
				if(s == nodes.get(start))
				{
					break;
				}
				g = s;
			}
			
			while (!stack.isEmpty()) {
				pathNodes.add(stack.pop());
			}
			
//			for(GeographicPoint gPoint: pathNodes)    //for debuging
//			{
//				System.out.printf("%1$s %2$s \n", gPoint.x,gPoint.y);
//			}		
			return pathNodes;
		}
		else {
			return null;
		}
	}
	

	/** Find the path from start to goal using Dijkstra's algorithm
	 * 
	 * @param start The starting location
	 * @param goal The goal location
	 * @return The list of intersections that form the shortest path from 
	 *   start to goal (including both start and goal).
	 */
	public List<GeographicPoint> dijkstra(GeographicPoint start, GeographicPoint goal) {
		// Dummy variable for calling the search algorithms
		// You do not need to change this method.
        Consumer<GeographicPoint> temp = (x) -> {};
        return dijkstra(start, goal, temp);
	}
	
	
	private List<GeographicPoint> reversePaht(HashMap<Intersection, Intersection> parentMap, GeographicPoint start, GeographicPoint goal) {
		List<GeographicPoint> pathNodes = new ArrayList<>();
		Intersection g = nodes.get(goal);              
		Stack<GeographicPoint> stack = new Stack<>();    //use stack to inverse the hop sequence.
		stack.push(g.getLoacation());
		while (true) {
			Intersection s = parentMap.get(g);
			stack.push(s.getLoacation());
			if(s == nodes.get(start))
			{
				break;
			}
			g = s;
		}
		
		while (!stack.isEmpty()) {
			pathNodes.add(stack.pop());
		}
		return pathNodes;
	}
	
	
	/** Find the path from start to goal using Dijkstra's algorithm
	 * 
	 * @param start The starting location
	 * @param goal The goal location
	 * @param nodeSearched A hook for visualization.  See assignment instructions for how to use it.
	 * @return The list of intersections that form the shortest path from 
	 *   start to goal (including both start and goal).
	 */
	public List<GeographicPoint> dijkstra(GeographicPoint start, 
										  GeographicPoint goal, Consumer<GeographicPoint> nodeSearched)
	{

		// Hook for visualization.  See writeup.
		//nodeSearched.accept(next.getLocation());
		
		
		PriorityQueue<DistanceInGraph> queue=new PriorityQueue<>(new DistanceInGraphComparator());
		Set<Intersection> visited = new HashSet<>();
		HashMap<Intersection, Intersection> parentMap = new HashMap<>();
		HashMap<GeographicPoint, Double> distanceMap = new HashMap<>();
		
		for (Map.Entry<GeographicPoint, Intersection> entry : nodes.entrySet()) {
			if (entry.getKey().equals(start)) {
				distanceMap.put(entry.getKey(), 0.0);
			}
			else {
				distanceMap.put(entry.getKey(), Double.POSITIVE_INFINITY);
			}
			
		}
		
		int count =0;
		
		
		queue.add(new DistanceInGraph(start, 0));
		boolean found =false;
		while (!queue.isEmpty()) {
			DistanceInGraph currDistance = queue.poll();
			count ++; //for quiz
			Intersection curr = nodes.get(currDistance.getPoint());
			nodeSearched.accept(curr.getLoacation());	// visualization
			if (!visited.contains(curr)) {
				visited.add(curr);
				if (curr.equals(nodes.get(goal))) {
					found = true;
					break;
				}
				
				
				double toCurrCost = distanceMap.get(curr.getLoacation());
				for(Intersection n:curr.neighbors())
				{
					double toNCost = distanceMap.get(n.getLoacation());
					
					
					if (!visited.contains(n)) {
						double alt = toCurrCost + curr.getTravelTimeTo(n.getLoacation()); //modified for duration
						
						if ( alt < toNCost) {
							distanceMap.put(n.getLoacation(),alt);
							parentMap.put(n, curr);
							queue.add(new DistanceInGraph(n.getLoacation(), alt));
						}						
					}
				}			
			}			
		}
		System.out.println("Dijkstra Count:"+String.valueOf(count)); //for quiz
		if (found) {
			
			return reversePaht(parentMap, start, goal);
		}
		else {
			return null;
		}
		
		
		
		
	}
	

	

	/** Find the path from start to goal using A-Star search
	 * 
	 * @param start The starting location
	 * @param goal The goal location
	 * @return The list of intersections that form the shortest path from 
	 *   start to goal (including both start and goal).
	 */
	public List<GeographicPoint> aStarSearch(GeographicPoint start, GeographicPoint goal) {
		// Dummy variable for calling the search algorithms
        Consumer<GeographicPoint> temp = (x) -> {};
        return aStarSearch(start, goal, temp);
	}
	
	/** Find the path from start to goal using A-Star search
	 * 
	 * @param start The starting location
	 * @param goal The goal location
	 * @param nodeSearched A hook for visualization.  See assignment instructions for how to use it.
	 * @return The list of intersections that form the shortest path from 
	 *   start to goal (including both start and goal).
	 */
	public List<GeographicPoint> aStarSearch(GeographicPoint start, 
											 GeographicPoint goal, Consumer<GeographicPoint> nodeSearched)
	{
		PriorityQueue<DistanceInGraph> queue=new PriorityQueue<>(new DistanceInGraphComparator());
		Set<Intersection> visited = new HashSet<>();
		HashMap<Intersection, Intersection> parentMap = new HashMap<>();
		HashMap<GeographicPoint, Double> distanceMap = new HashMap<>();
		HashMap<GeographicPoint, Double> toGoalMap = new HashMap<>();
		
		for (Map.Entry<GeographicPoint, Intersection> entry : nodes.entrySet()) {
			if (entry.getKey().equals(start)) {
				distanceMap.put(entry.getKey(), 0.0);
			}
			else {
				distanceMap.put(entry.getKey(), Double.POSITIVE_INFINITY);
			}
			double lat1 = entry.getKey().x - goal.x;
			double lon1 = entry.getKey().y - goal.y;
			
			double toGoal = Math.sqrt(lat1*lat1+lon1*lon1);
			
			
			toGoalMap.put(entry.getKey(), toGoal/120 ); //modified for duration.
		}
		int count=0;
		queue.add(new DistanceInGraph(start, 0));
		boolean found =false;
		while (!queue.isEmpty()) {
			DistanceInGraph currDistance = queue.poll();
			count++;// for quiz
			Intersection curr = nodes.get(currDistance.getPoint());
			nodeSearched.accept(curr.getLoacation());	// visualization
			if (!visited.contains(curr)) {
				visited.add(curr);
				if (curr.equals(nodes.get(goal))) {
					found = true;
					break;
				}
				
				
				double toCurrCost = distanceMap.get(curr.getLoacation());
				for(Intersection n:curr.neighbors())
				{
					double toNCost = distanceMap.get(n.getLoacation());
					
					
					if (!visited.contains(n)) {
						double alt = toCurrCost + curr.getTravelTimeTo(n.getLoacation()); //modified for duration
						
						if ( alt < toNCost) {
							distanceMap.put(n.getLoacation(),alt);
							parentMap.put(n, curr);
							queue.add(new DistanceInGraph(n.getLoacation(), alt+toGoalMap.get(n.getLoacation()))); // add heuristic distance to goal
						}						
					}
				}			
			}			
		}
		System.out.println("A* Count:"+String.valueOf(count));
		if (found) {
			
			return reversePaht(parentMap, start, goal);
		}
		else {
			return null;
		}
	}

	public void printGraph() {
		for (Map.Entry<GeographicPoint, Intersection> iterable_element : nodes.entrySet()) {
			GeographicPoint point = iterable_element.getKey();
			System.out.printf("%1$s,%2$s ==> ", point.x, point.y);
			for (Intersection i : iterable_element.getValue().neighbors()) {
				System.out.printf("%1$s,%2$s \t", i.getLoacation().x, i.getLoacation().y);
			}
			System.out.print("\n");
		}
	}
	
	public void printPath(List<GeographicPoint> path) {
		for(GeographicPoint geographicPoint: path){
			System.out.printf("%1$s,%2$s\n", geographicPoint.x,geographicPoint.y);
		}
		
		for(int i=0;i<path.size()-1;i++)
		{
			Intersection nIntersection = nodes.get(path.get(i));
			double distance = nIntersection.getTravelTimeTo(path.get(i+1));
			System.out.printf("%1$s,%2$s  --> %3$s,%4$s   %5$s\t", path.get(i).x,path.get(i).y,path.get(i+1).x,path.get(i+1).y,distance);
			System.out.print("[");
			for(Intersection intersection: nodes.get(path.get(i)).neighbors())
			{
				System.out.printf("%1$s,%2$s:%3$s\t\t",intersection.getLoacation().x,intersection.getLoacation().y,
						nodes.get(path.get(i)).getTravelTimeTo(intersection.getLoacation())); //modified for duration
			}
			
			System.out.print("]\n");
		}
		
	}
	
	
	
	
	
	public static void main(String[] args)
	{
		
		System.out.print("Making a new map...");
		MapGraph theMap = new MapGraph();
		System.out.print("DONE. \nLoading the map...");
		String map2 = "data/graders/mod3/map2.txt";
		String simpletest = "data/testdata/simpletest.map";
		String ucsd = "data/graders/mod3/ucsd.map";
		
		GraphLoader.loadRoadMap(ucsd, theMap);
		System.out.println("DONE.");
		//theMap.printGraph();
		GeographicPoint start = new GeographicPoint(32.8709815, -117.2434254);
		GeographicPoint goal = new GeographicPoint(32.8742087, -117.2381344);
		
		//theMap.bfs(start, goal);
		theMap.printPath(theMap.dijkstra(start, goal));
		
		// You can use this method for testing.  
		
		
//		MapGraph theMap = new MapGraph();
//		System.out.print("DONE. \nLoading the map...");
//		GraphLoader.loadRoadMap("data/maps/utc.map", theMap);
//		System.out.println("DONE.");
//
//		GeographicPoint start = new GeographicPoint(32.8648772, -117.2254046);
//		GeographicPoint end = new GeographicPoint(32.8660691, -117.217393);
//		
//		
//		List<GeographicPoint> route = theMap.dijkstra(start,end);
//		List<GeographicPoint> route2 = theMap.aStarSearch(start,end);

		
		
	}
	
}

class DistanceInGraphComparator implements Comparator<DistanceInGraph>
{
	public int compare(DistanceInGraph o1, DistanceInGraph o2) {
		
		return (int)Math.ceil(o1.getDistance()-o2.getDistance()) ;
	}	
}


/**
 * for storing distance starting from specific node to "to"
 * @author luning
 *
 */

class DistanceInGraph
{
	private double length;
	private GeographicPoint point;
	
	public DistanceInGraph(GeographicPoint to) {
		this(to, Double.POSITIVE_INFINITY);
	}
	
	public DistanceInGraph(GeographicPoint to, double length) {
		this.length = length;
		this.point = to;
	}
	
	public double getDistance() {
		return length;
	}
	
    public GeographicPoint getPoint() {
		return point;
	}
	
    public boolean equals(Object object)
    {
    	if (!(object instanceof DistanceInGraph)) {
			return false;
		}else {
			DistanceInGraph distanceInGraph = (DistanceInGraph) object;
			if (distanceInGraph.getPoint().equals(this.point) && Double.compare(distanceInGraph.getDistance()
					, this.length) == 0) {
				return true;
			}else {
				return false;
			}
		}
    }
		
}


